/*
 * mox_slave_c.c
 *
 *  Created on: Jul 11, 2010
 *      Author: jmonk
 */
#include <xs1.h>
#include <print.h>
#include <stdlib.h>
#include <stdio.h>
#include <gcc/stdarg.h>
#include "mox.h"
#include "mox_comm.h"
#include "mox_slave.h"
#include "mox_storage.h"
#include "mox_module.h"
#include "mox_log.h"
#include "mox_threads.h"
#ifdef FLASHER
#include "mox_flasher.h"
#endif

//Native module for startup goes here
//#include "module_uart.h"

// BAD!!!
void mox_create_chan_local(chanend *a, chanend *b);

void mox_slave(chanend master)
{
	struct mox_slave_state state;

	state.mast = master;
	init_slave(&state);
#ifndef FLASHER
	if (state.core_id == THD_START_CORE)
		transmit_func(state.mast);
#endif

	slave_log(&state, "Slave intialized Successfully\n");
	slave_server(&state, state.mast);

	return;
}


void slave_server(mox_slave_state *state, chanend mast)
{
	mox_msg_t tmp;
	while (1) {
		while (!mox_testct(mast)) mox_chan_in(mast);
		tmp = mox_inct(mast);
		if (tmp == MOX_MSG) {
			slave_handle(state);
		} else {
			//mox_chan_in_wd(mast);
			mox_chan_out_wd(mast, MOX_ACK);
			slave_service_thds(state);
		}
	}
}

void slave_handle_mod(unsigned st, int i, unsigned msg)
{
	mox_slave_state *state = (mox_slave_state *)st;
	switch (msg) {
	default:
		state->noresp[i] = 0;
	}

	return;
}

void slave_handle_thd(unsigned st, int i, unsigned msg_in)
{
	mox_slave_state *state = (mox_slave_state *)st;
	unsigned thread_id;
	unsigned index;
	unsigned temp;
	unsigned content[3];
	unsigned char *space;
	mox_msg *msg, *msg2;

	if (msg_in == MOX_MSG) {
		msg2 = (mox_msg *)mox_chan_in_wd(state->threads[i]);
		msg = malloc(sizeof(mox_msg));
		if (!msg) return;
		msg->content = msg2->content;
		msg2->content = NULL;
		msg->type = msg2->type;
		msg->size = msg2->size;
		msg2 = NULL;
		mox_chan_out_wd(state->threads[i], TH_ACK);
		msg_in = msg->type;
	}

	switch (msg_in) {
	case S_FREE:
		s_free(state->threads[i]);
		return;
	case S_MALLOC:
		s_malloc(state->threads[i]);
		return;
	/*case LOG_MESSAGE:
		chan_out(state->threads[i], TH_ACK);
		chan_out(state->log->channel, LOG_MESSAGE);
		while ((temp = chan_in(state->threads[i])) != LOG_END) chan_out(state->log->channel, temp);
		chan_out(state->log->channel, LOG_END);
		break;*/
	case TH_REQ_COM:
		content[0] = ((unsigned *)msg->content)[0];
		if (thread_is_local(&state, content[0])) {
			index = get_thd_index(&state, content[0]);
			if (index >= 7) {
				mox_comm_connect(state->threads[i], state->threads[index - 7], 1);
			} else {
				mox_comm_connect(state->threads[i], state->mod[index], 1);
			}
		} else {
		/*	while (chan_in(state->mast) != MOX_UPDATE) chan_out(state->mast, MOX_NACK);
			chan_out(state->mast, TH_REQ_COM);
			chan_out(state->mast, thread_id);
			if (chan_in(state->mast != TH_ACK)) {
				chan_out(state->threads[i], TH_NACK);
			} else {
				chan_out(state->threads[i], TH_ACK);
				chanend_connect(state->threads[i], state->mast);
			}*/
		}
		break;
	case TH_START_MOD:
	case TH_START_APP:
		while (1) {
			while (!mox_testct(state->mast)) mox_chan_in(state->mast);
			if (mox_inct(state->mast) == MOX_MSG) {
				mox_comm_msg_recv(state->mast);
				temp = msg->type;
				msg->type = MOX_NACK;
				mox_chan_out_wd(state->mast, MOX_MSG);
				mox_comm_msg_send(state->mast, *msg);
				msg->type = temp;
			} else {
				break;
			}
		}
		content[0] = state->thd_ids[7 + i];
		content[1] = ((unsigned *)msg->content)[0];
		content[2] = ((unsigned *)msg->content)[1];
		free(msg->content);
		msg->content = (unsigned char *)content;
		msg->size = 3 * sizeof(unsigned);
		mox_chan_out_wd(state->mast, MOX_MSG);
		mox_comm_msg_send(state->mast, *msg);
		break;
	case TH_REQ_CFG:
	case TH_SET_CFG:
		while (1) {
			while (!mox_testct(state->mast)) mox_chan_in(state->mast);
			if (mox_inct(state->mast) == MOX_MSG) {
				mox_comm_msg_recv(state->mast);
				temp = msg->type;
				msg->type = MOX_NACK;
				mox_chan_out_wd(state->mast, MOX_MSG);
				mox_comm_msg_send(state->mast, *msg);
				msg->type = temp;
			} else {
				break;
			}
		}

		msg->size += 4;
		space = (unsigned char *)malloc(msg->size);
		if (!space) return;

		((unsigned *)space)[0] = state->thd_ids[7 + i];
		for (temp = 0; temp < (msg->size - 4); temp++) {
			space[temp + 4] = msg->content[temp];
		}

		mox_chan_out_wd(state->mast, MOX_MSG);
		mox_comm_msg_send(state->mast, *msg);
		break;
	case TH_CON_REQ:
	case TH_DIS_REQ:
		while (1) {
			while (!mox_testct(state->mast)) mox_chan_in(state->mast);
			if (mox_inct(state->mast) == MOX_MSG) {
				mox_comm_msg_recv(state->mast);
				temp = msg->type;
				msg->type = MOX_NACK;
				mox_chan_out_wd(state->mast, MOX_MSG);
				mox_comm_msg_send(state->mast, *msg);
				msg->type = temp;
			}	else {
				break;
			}
		}

		content[0] = ((unsigned *)msg->content)[0];
		content[1] = state->thd_ids[i];
		content[2] = ((unsigned *)msg->content)[1];
		free(msg->content);
		msg->content = (unsigned char *)content;
		msg->size = 3 * sizeof(unsigned);

		mox_chan_out_wd(state->mast, MOX_MSG);
		mox_comm_msg_send(state->mast, *msg);
		break;
	case TH_LST_REQ:
	case TH_DTL_REQ:
		while (1) {
			while (!mox_testct(state->mast)) mox_chan_in(state->mast);
			if (mox_inct(state->mast) == MOX_MSG) {
				mox_comm_msg_recv(state->mast);
				temp = msg->type;
				msg->type = MOX_NACK;
				mox_comm_msg_send(state->mast, *msg);
				msg->type = temp;
			} else {
				break;
			}
		}

		content[0] = state->thd_ids[i];
		if (msg->content) {
			content[1] = ((unsigned *)msg->content)[0];
			free(msg->content);
		}
		msg->content = (unsigned char *)content;
		msg->size = 2 * sizeof(unsigned);

		mox_chan_out_wd(state->mast, MOX_MSG);
		mox_comm_msg_send(state->mast, *msg);
		break;
	}
	state->noresp[i + 7] = 0;

	return;
}

int get_thd_index(struct mox_slave_state *state, unsigned thread_id)
{
	int i;
	for (i = 0; i < 14; i++) {
		if (thread_id == state->thd_ids[i]) return (i);
	}
	return -1;
}

int thread_is_mod(struct mox_slave_state *state, unsigned thread_id)
{
	int i;
	for (i = 0; i < 7; i++) {
		if (thread_id == state->thd_ids[i]) return 1;
	}

	return 0;
}

int thread_is_local(struct mox_slave_state *state, unsigned thread_id)
{
	int i;
	for (i = 0; i < 14; i++) {
		if (thread_id == state->thd_ids[i]) return 1;
	}

	return 0;
}

void slave_handle(mox_slave_state *state)
{
	unsigned temp;
	mox_msg *msg = mox_comm_msg_recv(state->mast);

	if (!msg) return;
	switch (msg->type) {
	case MOX_CMD:
		handle_cmd(state, msg);
		break;
	case MOX_CREATE_TH:
		slave_log(state, "Create Thread");

		if (slave_create_thread(state, msg)) {
			slave_log(state, "Error: Slave failed to create thread");
		}
		break;
	case MOX_REQ_THD:
		master_thd_connect(state, msg);
		break;
	case MOX_INIT_CRTH:
		slave_log(state, "Init Create");
		if (init_mod_start(state, msg)) {
			slave_log(state, "Error: Slave failed to create init thread\n");
		} else {
			slave_log(state, "Success: Slave successfully created a thread\n");
		}
		break;
#ifdef FLASHER
	case FLASH_GET_FUNC:
		flasher_slave(state);
		break;
#endif
	default:
		mox_chan_out_wd(state->mast, MOX_MSG);
		msg->type = MOX_NACK;
		mox_comm_msg_send(state->mast, *msg);
		slave_log(state, "Warning: Slave Received Unknown Message from Master\n");
		break;
	}

	if (msg->content) free(msg->content);
	free(msg);

	slave_service_thds(state);

	return;
}

void slave_service_thds(mox_slave_state *state)
{
	int i;
	slave_clear_chans(state->mod, state->num_mod);
	slave_clear_chans(state->threads, state->num_thd);
	for (i = 0; i < 14; i++) {
		state->noresp[i]++;
	}
	slave_snd_rcv(state->mod, state->num_thd, MOD_ACK, state->threads, state->num_thd, TH_ACK, (int)state);

	return;
}

void slave_clear_chans(chanend chans[], unsigned n)
{
	int i;
	for (i = 0; i < n; i++) {
		mox_chan_in_noblock(chans[i]);
	}
	for (i = 0; i < n; i++) {
		mox_chan_in_noblock(chans[i]);
	}
}

void handle_cmd(struct mox_slave_state *state, mox_msg *msg)
{
	mox_cmd_t temp = 0;
	chanend a[3];

	if (msg->content) {
		temp = ((unsigned *)msg->content)[0];
	}
	if ((temp != MOX_ACK) && (temp != MOX_NACK)) {
		switch (temp) {
		case LOG_UPDATE:
			temp = ((unsigned *)msg->content)[1];
			state->log = (struct system_log *)malloc(sizeof(struct system_log));
			state->log->channel = temp;
			state->log->core_id = ((unsigned *)msg->content)[2];
			break;
		case CHAN_CREATE_LOC:
			mox_create_chan_local(&a[1], &a[2]);
			free(msg->content);
			a[0] = CHAN_CREATE_LOC;
			msg->content = (unsigned char *)a;
			msg->size = 2 * sizeof(chanend);
			msg->type = MOX_ACK;

			mox_comm_msg_send(state->mast, *msg);
			msg->content = NULL;
			break;
		case CHAN_CREATE_REM:
			temp = ((unsigned *)msg->content)[1];
			asm("getr %0,0x2":"=r"(a[0]):/*no inputs*/);
			if (temp) {
				msg->type = MOX_ACK;
				msg->size = sizeof(chanend);
				((unsigned *)msg->content)[0] = CHAN_CREATE_REM;
				((unsigned *)msg->content)[1] = a[0];
				mox_comm_msg_send(state->mast, *msg);
				free(msg->content);
				free(msg);
				mox_inct(state->mast);
				msg = mox_comm_msg_recv(state->mast);
				if (msg && (msg->type == MOX_ACK) && msg->content && (((unsigned *)msg->content)[0] == CHAN_CREATE_REM)) {
					a[1] = ((unsigned *)msg->content)[1];
				}
			} else {
				mox_inct(state->mast);
				msg = mox_comm_msg_recv(state->mast);
				if (msg && (msg->type == MOX_ACK) && msg->content && (((unsigned *)msg->content)[0] == CHAN_CREATE_REM)) {
					a[1] = ((unsigned *)msg->content)[1];
				}
				msg->type = MOX_ACK;
				msg->size = sizeof(chanend);
				((unsigned *)msg->content)[0] = CHAN_CREATE_REM;
				((unsigned *)msg->content)[1] = a[0];
				mox_comm_msg_send(state->mast, *msg);
				free(msg->content);
				free(msg);
			}
			asm("setd res[%0],%1":/*no outputs*/:"r"(a[0]), "r"(a[1]));
			break;
		default:
			msg->type = MOX_NACK;
			mox_comm_msg_send(state->mast, *msg);
			break;
		}
	}

	if (msg->content) free(msg->content);
	free(msg);


	return;
}

int init_mod_start(struct mox_slave_state *state, mox_msg *msg)
{
	int *prog_space;
	chanend mox, mod;
	int stack_size, *stack_space;
	int const_addr;
	int data_addr;

	prog_space = ((int **)msg->content)[0];
	stack_size = ((int *)msg->content)[1]+0x100;
	while (!(stack_space = malloc(stack_size))) {
		slave_log(state, "Warning: Slave Init Module is Unexpectedly Reducing Stack Space\n");
		stack_size /= 2;
	}

	mox_create_chan_local(&mox, &mod);

	thread_create(prog_space, mox, stack_space+0x100,
				const_addr+prog_space, data_addr+prog_space, 0);


	return mod_thd_start_release(mod, state);
}

void master_thd_connect(struct mox_slave_state *state, mox_msg *msg)
{
	unsigned id = ((unsigned *)msg->content)[0];
	unsigned index = get_thd_index(state, id);

	msg->type = MOX_ACK;
	mox_comm_msg_send(state->mast, *msg);

	if (index < 7) {
		mox_comm_connect(state->mast, state->mod[index], 1);
	} else {
		mox_comm_connect(state->mast, state->threads[index - 7], 1);
	}

	return;
}

int slave_create_thread(struct mox_slave_state *state, mox_msg *msg)
{
	int const_addr;
	int data_addr;
	int size;
	int stack_size;
	int offset;
	int *prog_space;
	int *stack_space;
	chanend mox, mod;
	int i;

	// TODO: Add verification that first message in sequence

	size = ((int *)msg->content)[0];
	const_addr = ((int *)msg->content)[1];
	data_addr = ((int *)msg->content)[2];
	stack_size = ((int *)msg->content)[3]+0x200;
	offset = ((int *)msg->content)[4];

	free(msg->content);
	free(msg);
	mox_inct(state->mast);
	msg = mox_comm_msg_recv(state->mast);
	if (!msg) slave_log(state, "Warning: Slave Unable to Allocate Thread Space\n");
	prog_space = msg->content;
	data_addr = (int)(data_addr + (char *)prog_space);
	const_addr = (int)(const_addr + (char *)prog_space);
	prog_space = (int *)((char*)prog_space + offset);


	while (!(stack_space = malloc(stack_size))) {
		slave_log(state, "Warning: Slave Thread Create is Unexpectedly Reducing Stack Space\n");
		stack_size /= 2;
	}

	mox_create_chan_local(&mox, &mod);

	thread_create(prog_space, mox, stack_space+0x200,
			(int *)const_addr, (int *)data_addr, 1);

	return mod_thd_start_release(mod, state);
}

void init_slave(struct mox_slave_state *state)
{
	int i;
	state->log = NULL;
	state->active = 0;

	for (i = 0; i < 7; i++) {
		state->mod[i] = 0;
		state->threads[i] = 0;
		state->thd_ids[i] = -1;
		state->thd_ids[i+7] = -1;
	}
	state->num_mod = 0;
	state->num_thd = 0;			//need to rename to specify the difference
	state->num_threads = 0;

	state->core_id = mox_chan_in_wd(state->mast);
	mox_chan_out_wd(state->mast, MOX_ACK);
	slave_log(state, "Slave Enumerated Successfully %d\n", state->core_id);

	return;
}

#ifndef FLASHER

void THD_START_FUNC(void);

void transmit_func(chanend mast)
{
	// put your startup module/thread here
	void *func = THD_START_FUNC;

	mox_chan_out_wd(mast, (int)func);

	return;
}
#endif


void s_malloc(chanend c)
{
	unsigned tmp;
	void *p;
	tmp = mox_chan_in_wd(c);
	p = malloc(tmp);
	mox_chan_out_wd(c, (int)p);
	return;
}

void s_free(chanend c)
{
	unsigned tmp;
	void *p;
	p = (void *)mox_chan_in_wd(c);
	free(p);
	return;
}

void slave_handle_ct(chanend channel, unsigned tmp)
{
	switch (tmp) {
	case S_FREE:
		s_free(channel);
		break;
	case S_MALLOC:
		s_malloc(channel);
		break;
	}

	return;
}
